/******************************************************************************
 * Copyright 2022 The SANY Autonomous Driving Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

/*
 * @file
 */

#include "cyber/common/file.h"
#include "modules/common/math/box2d.h"
#include "modules/common/math/vec2d.h"
#include "modules/common/proto/chassis.pb.h"
#include "modules/common/vehicle_state/vehicle_state_provider.h"
#include "modules/planning/common/obstacle.h"
#include "modules/planning/common/path/discretized_path.h"
#include "modules/planning/common/util/util.h"
#include "modules/planning/gflags/planning_gflags.h"
#include "modules/planning/math/discrete_points_math.h"
#include "modules/planning/open_space/coarse_trajectory_generator/hybrid_a_star.h"
#include "modules/planning/open_space/trajectory_smoother/iterative_anchoring_smoother.h"
#include "modules/tools/test/CppRobotics/hybrid_test/proto/hybrid_parms.pb.h"
#include "modules/tools/test/CppRobotics/osqp_test/src/common/smoothing/matplotlibcpp.h"
#include "gtest/gtest.h"

namespace apollo {
namespace planning {

using apollo::common::VehicleStateProvider;
using apollo::common::math::Box2d;
using apollo::common::math::Vec2d;
using apollo::planning::DiscretizedPath;
using apollo::test::HybridAStarParm;

typedef std::pair<DiscretizedPath, common::Chassis::GearPosition> PathGearPair;

namespace plt = matplotlibcpp;

class HybridATest : public ::testing::Test {
public:
  virtual void SetUp() {
    // FLAGS_planner_open_space_config_filename =
    //     "/apollo/modules/planning/testdata/conf/"
    //     "open_space_standard_parking_lot.pb.txt";

    if (!apollo::cyber::common::GetProtoFromFile(
            FLAGS_planner_open_space_config_filename,
            &planner_open_space_config_)) {
      AERROR << "Failed to load open space config file "
             << FLAGS_planner_open_space_config_filename;
    }

    std::string hybrid_file_name =
        "/apollo/modules/tools/test/CppRobotics/hybrid_test/conf/"
        "hybrid_parms.pb.txt";
    if (!apollo::cyber::common::GetProtoFromFile(hybrid_file_name,
                                                 &hybrid_parms_)) {
      AERROR << "Failed to load hybrid file " << hybrid_file_name;
    }

    // hybrid a star 算法测试
    hybrid_test = std::unique_ptr<HybridAStar>(
        new HybridAStar(planner_open_space_config_));

    // 离散点平滑算法测试
    iterative_anchoring_smoosher_.reset(
        new IterativeAnchoringSmoother(planner_open_space_config_));
  }

protected:
  std::unique_ptr<HybridAStar> hybrid_test;
  PlannerOpenSpaceConfig planner_open_space_config_;
  std::unique_ptr<IterativeAnchoringSmoother> iterative_anchoring_smoosher_;
  HybridAStarParm hybrid_parms_;
};

bool GenerateDiscretizedPath(const HybridAStartResult &result,
                             DiscretizedPath *smoothed_path_points) {

  if (nullptr == smoothed_path_points) {
    std::cout << "smoothed_path_points is nullptr.";
    return false;
  }
  if (result.x.size() != result.y.size() ||
      result.x.size() != result.phi.size() ||
      result.y.size() != result.phi.size()) {
    return false;
  }

  std::vector<double> headings;
  std::vector<double> accumulated_s;
  std::vector<double> kappas;
  std::vector<double> dkappas;
  std::vector<std::pair<double, double>> record_path_point2ds;
  for (size_t i = 0; i != result.x.size(); ++i) {
    record_path_point2ds.emplace_back(result.x[i], result.y[i]);
  }
  if (!DiscretePointsMath::ComputePathProfile(
          record_path_point2ds, &headings, &accumulated_s, &kappas, &dkappas)) {
    std::cout << "Failure to compute path profile.";
    return false;
  }

  size_t result_size = result.x.size();
  if (result_size == accumulated_s.size() && result_size == kappas.size() &&
      result_size == dkappas.size()) {
    for (size_t i = 0; i < result_size; ++i) {
      common::PathPoint path_point;
      path_point.set_x(result.x[i]);
      path_point.set_y(result.y[i]);
      if (result.gear) {
        path_point.set_theta(headings[i]);
      } else {
        path_point.set_theta(common::math::NormalizeAngle(headings[i] + M_PI));
      }
      path_point.set_s(accumulated_s[i]);
      path_point.set_kappa(kappas[i]);
      path_point.set_dkappa(dkappas[i]);
      smoothed_path_points->push_back(std::move(path_point));
    }
  }
  return true;
}

void AdjustKappaByGear(DiscretizedPath *discretized_path, const bool gear) {
  if (gear) {
    return;
  }
  for (auto &point : *discretized_path) {
    point.set_kappa(-1.0 * point.kappa());
  }
}

bool TrajectoryPartition(const HybridAStartResult &result,
                         std::vector<HybridAStartResult> *partitioned_result) {
  const auto &x = result.x;
  const auto &y = result.y;
  const auto &theta = result.phi;
  if (x.size() != y.size() || x.size() != theta.size()) {
    ADEBUG << "states sizes are not equal when do trajectory partitioning of"
              "Hybrid A Star result";
    return false;
  }

  if (x.size() < 4) {
    return false;
  }

  size_t horizon = x.size();
  ADEBUG << "Size of origin trajectory[" << horizon << "].";
  partitioned_result->clear();
  partitioned_result->emplace_back();
  auto *current_traj = &(partitioned_result->back());
  double heading_angle = theta.front();
  const common::math::Vec2d init_tracking_vector(x[1] - x[0], y[1] - y[0]);
  double tracking_angle = init_tracking_vector.Angle();
  bool current_gear =
      std::abs(common::math::NormalizeAngle(tracking_angle - heading_angle)) <
      M_PI_2;
  current_traj->gear = current_gear;
  ADEBUG << "Initial gear["
         << common::Chassis::GearPosition_Name(current_gear).c_str() << "].";

  for (size_t i = 0; i < horizon - 1; ++i) {
    heading_angle = theta[i];
    const Vec2d tracking_vector(x[i + 1] - x[i], y[i + 1] - y[i]);
    tracking_angle = tracking_vector.Angle();
    bool gear =
        std::abs(common::math::NormalizeAngle(tracking_angle - heading_angle)) <
        (M_PI_2);
    if (gear != current_gear) {
      current_traj->x.push_back(x[i]);
      current_traj->y.push_back(y[i]);
      current_traj->phi.push_back(theta[i]);
      partitioned_result->emplace_back();
      current_traj = &(partitioned_result->back());
      current_traj->gear = gear;
      current_gear = gear;
      AINFO << "Gear of other path[" << current_gear << "].";
    }
    current_traj->x.push_back(x[i]);
    current_traj->y.push_back(y[i]);
    current_traj->phi.push_back(theta[i]);
    AINFO << "Point[" << i << "]: x[" << x[i] << "], y[" << y[i] << "], theta["
          << theta[i] << "].";
  }
  current_traj->x.push_back(x.back());
  current_traj->y.push_back(y.back());
  current_traj->phi.push_back(theta.back());

  return true;
}

void LoadResult(const DiscretizedTrajectory &discretized_trajectory,
                Eigen::MatrixXd *state_result_dc,
                Eigen::MatrixXd *control_result_dc,
                Eigen::MatrixXd *time_result_dc) {
  const size_t points_size = discretized_trajectory.size();

  *state_result_dc = Eigen::MatrixXd::Zero(4, points_size);
  *control_result_dc = Eigen::MatrixXd::Zero(2, points_size - 1);
  *time_result_dc = Eigen::MatrixXd::Zero(1, points_size - 1);

  auto &state_result = *state_result_dc;
  for (size_t i = 0; i < points_size; ++i) {
    state_result(0, i) = discretized_trajectory[i].path_point().x();
    state_result(1, i) = discretized_trajectory[i].path_point().y();
    state_result(2, i) = discretized_trajectory[i].path_point().theta();
    state_result(3, i) = discretized_trajectory[i].v();
  }

  auto &control_result = *control_result_dc;
  auto &time_result = *time_result_dc;
  const double wheel_base = common::VehicleConfigHelper::Instance()
                                ->GetConfig()
                                .vehicle_param()
                                .wheel_base();
  for (size_t i = 0; i + 1 < points_size; ++i) {
    control_result(0, i) =
        std::atan(discretized_trajectory[i].path_point().kappa() * wheel_base);
    control_result(1, i) = discretized_trajectory[i].a();
    time_result(0, i) = discretized_trajectory[i + 1].relative_time() -
                        discretized_trajectory[i].relative_time();
  }
}

TEST_F(HybridATest, test1) {

  // 初始状态设置(x,y,theta)
  double sx = hybrid_parms_.start_point().x();
  double sy = hybrid_parms_.start_point().y();
  double sphi = hybrid_parms_.start_point().heading();
  // 终点状态设置
  double ex = hybrid_parms_.end_point().x();
  double ey = hybrid_parms_.end_point().y();
  double ephi = hybrid_parms_.end_point().heading(); // rad
  // // 初始状态设置(x,y,theta)
  // double sx = -15.0;
  // double sy = 5.0;
  // double sphi = 0.0;
  // // 终点状态设置
  // double ex = 2.5;
  // double ey = -8;
  // double ephi = 90.0 / 57.3; // rad
  std::vector<std::vector<Vec2d>> obstacles_list;
  HybridAStartResult result;
  GridAStartResult astar_result;

  // 障碍物角点(x,y)，逆时针排列
  // AINFO<<"---------------"<<hybrid_parms_.obstacle_vertices().obstacle_vertice_size();
  for (auto obs : hybrid_parms_.obstacle_vertices().obstacle_vertice()) {
    std::vector<Vec2d> obstacle;
    for (auto ver : obs.vertice()) {
      Vec2d obstacle_vertice_bo(ver.x(), ver.y());
      obstacle.push_back(obstacle_vertice_bo);
    }
    obstacles_list.emplace_back(obstacle);
  }

  // bound构建
  for (auto obs : hybrid_parms_.bounds().bound()) {
    std::vector<Vec2d> obstacle;
    for (auto ver : obs.vertice()) {
      Vec2d obstacle_vertice_bo(ver.x(), ver.y());
      obstacle.push_back(obstacle_vertice_bo);
    }
    obstacles_list.emplace_back(obstacle);
  }

  // Vec2d obstacle_vertice_ao(-5.0, 9);
  // Vec2d obstacle_vertice_bo(-5.0, 0.0);
  // std::vector<Vec2d> obstacle = {obstacle_vertice_ao, obstacle_vertice_bo};
  // obstacles_list.emplace_back(obstacle);

  // 限定搜索边界也可以通过障碍物进行构建
  // 构造上边界
  // Vec2d obstacle_vertice_a_u(35.0, 35.0);
  // Vec2d obstacle_vertice_b_u(-30.0, 35.0);
  // std::vector<Vec2d> up_bound = {obstacle_vertice_a_u, obstacle_vertice_b_u};
  // obstacles_list.emplace_back(up_bound);
  // 构造下边界
  // Vec2d obstacle_vertice_a(35.0, 0.0);
  // Vec2d obstacle_vertice_b(3.1, 0.0);
  // Vec2d obstacle_vertice_c(3.1, -30.0);
  // Vec2d obstacle_vertice_d(0.0, -30.0);
  // Vec2d obstacle_vertice_e(0.0, 0.0);
  // Vec2d obstacle_vertice_f(-30.0, 0.0);
  // std::vector<Vec2d> low_bound = {obstacle_vertice_a, obstacle_vertice_b,
  //                                 obstacle_vertice_c, obstacle_vertice_d,
  //                                 obstacle_vertice_e, obstacle_vertice_f};
  // obstacles_list.emplace_back(low_bound);

  // 获取障碍物外边界，有线段组成
  std::vector<std::vector<common::math::LineSegment2d>>
      obstacles_linesegments_vec;
  for (const auto &obstacle_vertices : obstacles_list) {
    size_t vertices_num = obstacle_vertices.size();
    std::vector<common::math::LineSegment2d> obstacle_linesegments;
    for (size_t i = 0; i < vertices_num - 1; ++i) {
      common::math::LineSegment2d line_segment = common::math::LineSegment2d(
          obstacle_vertices[i], obstacle_vertices[i + 1]);
      obstacle_linesegments.emplace_back(line_segment);
    }
    obstacles_linesegments_vec.emplace_back(obstacle_linesegments);
  }

  // load xy boundary into the Plan() from configuration(Independent from frame)
  // 用于构建整个网格搜索区域
  std::vector<double> XYbounds_;
  // XYbounds_.push_back(-30.0); // x_min
  // XYbounds_.push_back(35.0);  // x_max
  // XYbounds_.push_back(-10.0); // y_min
  // XYbounds_.push_back(11.0);  // y_max

  XYbounds_.push_back(-80.0); // x_min
  XYbounds_.push_back(80.0);  // x_max
  XYbounds_.push_back(-35.0); // y_min
  XYbounds_.push_back(35.0);  // y_max

  plt::figure();
  // 绘制障碍物边界
  for (const auto &obstacle_linesegments : obstacles_linesegments_vec) {
    for (const common::math::LineSegment2d &linesegment :
         obstacle_linesegments) {
      plt::plot(
          std::vector<double>{linesegment.start().x(), linesegment.end().x()},
          std::vector<double>{linesegment.start().y(), linesegment.end().y()},
          "-k");
    }
  }
  hybrid_test->AStarPlan(sx, sy, sphi, ex, ey, ephi, XYbounds_, obstacles_list,
                         &astar_result);
  hybrid_test->Plan(sx, sy, sphi, ex, ey, ephi, XYbounds_, 0.0, obstacles_list,
                    &result);

  // 将路径进行分段，并计算速度
  std::vector<HybridAStartResult> partition_trajectories;
  if (result.x.size()) {
    // 调用hybrid a star 分段路径，但有问题，档位没有赋值
    hybrid_test->TrajectoryPartition(result, &partition_trajectories);
    // 调用修改后的路径分段
    // TrajectoryPartition(result, &partition_trajectories);
  };

  const size_t size = partition_trajectories.size();

  std::vector<Eigen::MatrixXd> state_result_dc;
  std::vector<Eigen::MatrixXd> control_result_dc;
  std::vector<Eigen::MatrixXd> time_result_dc;
  state_result_dc.resize(size);
  control_result_dc.resize(size);
  time_result_dc.resize(size);

  std::vector<HybridAStartResult> partitioned_result(partition_trajectories);
  // smoothed path and speed:genetate speed.
  std::vector<PathGearPair> partitioned_paths_;
  HybridAStartResult smoothed_trajectory;
  double accumulated_s = 0;
  for (size_t i = 0; i != partitioned_result.size(); ++i) {
    std::cout << "partitioned_result:" << i << std::endl;
    DiscretizedPath smoothed_path_points;
    GenerateDiscretizedPath(partitioned_result[i], &smoothed_path_points);

    // GenerateHybridSmoothedPath(partitioned_result[i],
    //                            partitioned_result[i].gear,
    //                            &smoothed_path_points);

    constexpr double kEpsilon = 1e-6;
    const double param = kEpsilon;
    if (!iterative_anchoring_smoosher_->Smooth(
            partitioned_result[i], partitioned_result[i].gear, obstacles_list,
            &smoothed_path_points, param)) {
      std::cout << "smoothed the path failed.";
    }

    // DiscretizedTrajectory discretized_trajectory(
    //     iterative_anchoring_smoosher_->Get_discretized_trajectory());

    DiscretizedTrajectory discretized_trajectory =
        std::move(iterative_anchoring_smoosher_->Get_discretized_trajectory());

    LoadResult(discretized_trajectory, &state_result_dc[i],
               &control_result_dc[i], &time_result_dc[i]);

    auto last_x = discretized_trajectory[0].path_point().x();
    auto last_y = discretized_trajectory[0].path_point().y();
    for (size_t i = 0; i < discretized_trajectory.size(); ++i) {
      double x_diff = discretized_trajectory[i].path_point().x() - last_x;
      double y_diff = discretized_trajectory[i].path_point().y() - last_y;
      accumulated_s += std::sqrt(x_diff * x_diff + y_diff * y_diff);
      // AINFO << accumulated_s << ",i:" << i;
      last_x = discretized_trajectory[i].path_point().x();
      last_y = discretized_trajectory[i].path_point().y();
      smoothed_trajectory.accumulated_s.push_back(accumulated_s);
      smoothed_trajectory.x.push_back(
          discretized_trajectory[i].path_point().x());
      smoothed_trajectory.y.push_back(
          discretized_trajectory[i].path_point().y());
      smoothed_trajectory.phi.push_back(
          discretized_trajectory[i].path_point().theta());
      smoothed_trajectory.v.push_back(discretized_trajectory[i].v());
      smoothed_trajectory.a.push_back(discretized_trajectory[i].a());
    }

    if (!partitioned_result[i].gear) {
      for (size_t i = 0; i < smoothed_path_points.size(); i++) {
        smoothed_path_points[i].set_theta(common::math::NormalizeAngle(
            smoothed_path_points[i].theta() + M_PI));
      }
    }
    AdjustKappaByGear(&smoothed_path_points, partitioned_result[i].gear);

    auto &vehicle_pose =
        common::VehicleStateProvider::Instance()->original_pose();
    for (size_t i = 0; i < smoothed_path_points.size(); i++) {
      common::PathPoint enu_point;
      planning::util::FLUPointToENUPoint(vehicle_pose, smoothed_path_points[i],
                                         &enu_point);
      smoothed_path_points[i].set_enu_x(enu_point.x());
      smoothed_path_points[i].set_enu_y(enu_point.y());
      smoothed_path_points[i].set_enu_heading(enu_point.theta());
    }
    common::Chassis::GearPosition gear =
        partitioned_result[i].gear ? common::Chassis_GearPosition_GEAR_DRIVE
                                   : common::Chassis_GearPosition_GEAR_REVERSE;
    PathGearPair partitioned_path;
    partitioned_path.first = smoothed_path_points;
    partitioned_path.second = gear;
    partitioned_paths_.emplace_back(partitioned_path);
  }

  // 获取平滑后的路径
  HybridAStartResult result_smoothed;
  for (int i = 0; i < partitioned_paths_.size(); i++) {
    auto cur_path = partitioned_paths_.at(i).first;
    for (size_t j = 0; j < cur_path.size(); j++) {
      result_smoothed.x.push_back(cur_path.at(j).x());
      result_smoothed.y.push_back(cur_path.at(j).y());
      result_smoothed.phi.push_back(cur_path.at(j).theta());
    }
  }

  // 计算累计距离
  double s = 0;
  double last_s = 0;
  for (auto &p : partition_trajectories) {
    s += last_s;
    for (int i = 1; i < p.x.size(); i++) {
      p.accumulated_s.push_back(s);
      s += std::sqrt(std::pow(p.x[i] - p.x[i - 1], 2) +
                     std::pow(p.y[i] - p.y[i - 1], 2));
    }
    last_s = p.accumulated_s.back();
  }

  // ASSERT_TRUE(hybrid_test->Plan(sx, sy, sphi, ex, ey, ephi,
  // XYbounds_, 0.0,
  //                               obstacles_list, &result));

  // 车辆总体参数
  common::VehicleParam vehicle_param_ =
      common::VehicleConfigHelper::GetConfig().vehicle_param();

  // 绘制本车规划的未平滑路径
  for (int i = 0; i < result.x.size(); i++) {
    // 构造本车的位置和姿态
    // Box2d ego({result.x[i], result.y[i]}, result.phi[i], 11.0, 2.55);
    Box2d ego_box = Node3d::GetBoundingBox(vehicle_param_, result.x[i],
                                           result.y[i], result.phi[i]);
    std::vector<Vec2d> corners2;
    ego_box.GetAllCorners(&corners2);
    corners2.push_back(corners2.front()); //把第一个点作为最后一个点，形成闭环
    std::vector<double> c_x;
    std::vector<double> c_y;
    for (auto &c : corners2) {
      c_x.push_back(c.x());
      c_y.push_back(c.y());
    }
    if (i == 0) {
      plt::plot(c_x, c_y, "g-"); //起始位置
    } else if (i == result.x.size() - 1) {
      plt::plot(c_x, c_y, "r-"); //终止位置
    } else {
      plt::plot(c_x, c_y, "y-");
    }
  }

  // 补齐，使a和steer与x的数据长度相等
  result.a.push_back(0);
  result.steer.push_back(0);

  // 绘制本车规划的平滑路径
  for (int i = 0; i < result_smoothed.x.size(); i += 1) {
    // 构造本车的位置和姿态
    // Box2d ego({result.x[i], result.y[i]}, result.phi[i], 11.0, 2.55);
    Box2d ego_box =
        Node3d::GetBoundingBox(vehicle_param_, result_smoothed.x[i],
                               result_smoothed.y[i], result_smoothed.phi[i]);
    std::vector<Vec2d> corners2;
    ego_box.GetAllCorners(&corners2);
    corners2.push_back(corners2.front());
    //把第一个点作为最后一个点，形成闭环 
    std::vector<double> c_x;
    std::vector<double> c_y;
    for (auto &c : corners2) {
      c_x.push_back(c.x());
      c_y.push_back(c.y());
    }
    plt::plot(c_x, c_y, "c-"); //起始位置
  }

  // 绘制泊车区域

  plt::named_plot("result path", result.x, result.y, "b.-");
  plt::named_plot("result smoothed path", result_smoothed.x, result_smoothed.y,
                  "m.-");
  plt::named_plot("a star result path", astar_result.x, astar_result.y, "g.-");
  plt::named_plot("start position", std::vector<double>{sx},
                  std::vector<double>{sy}, "go");
  plt::named_plot("end position", std::vector<double>{ex},
                  std::vector<double>{ey}, "ro");
  plt::xlabel("x(m)");
  plt::ylabel("y(m)");
  plt::legend();
  plt::grid(true);
  plt::axis("equal");

  // plt::figure();
  // std::vector<double> v;
  // std::vector<double> a;
  // std::vector<double> phi;
  // for (auto &p : partition_trajectories) {
  //   v.insert(v.end(), p.v.begin(), p.v.end());
  //   a.insert(a.end(), p.a.begin(), p.a.end());
  //   phi.insert(phi.end(), p.phi.begin(), p.phi.end());
  // }
  // plt::subplot(3, 1, 1);
  // plt::plot(v, "b.-");
  // plt::grid(true);
  // plt::ylabel("v(m/s)");
  // plt::subplot(3, 1, 2);
  // plt::plot(a, "b.-");
  // plt::grid(true);
  // plt::ylabel("a(m/s^2)");
  // plt::subplot(3, 1, 3);
  // plt::plot(phi, "b.-");
  // plt::grid(true);
  // plt::ylabel("heading(rad)");

  plt::figure();
  plt::subplot(3, 1, 1);
  plt::named_plot("hybrid a star", result.accumulated_s, result.v, "b.-");
  plt::named_plot("smoothed", smoothed_trajectory.accumulated_s,
                  smoothed_trajectory.v, "r.-");
  plt::grid(true);
  plt::ylabel("v(m/s)");
  plt::legend();
  plt::subplot(3, 1, 2);
  plt::named_plot("hybrid a star", result.accumulated_s, result.a, "b.-");
  plt::named_plot("smoothed", smoothed_trajectory.accumulated_s,
                  smoothed_trajectory.a, "r.-");
  plt::grid(true);
  plt::ylabel("a(m/s^2)");
  plt::subplot(3, 1, 3);
  plt::named_plot("hybrid a star", result.accumulated_s, result.phi, "b.-");
  plt::named_plot("smoothed", smoothed_trajectory.accumulated_s,
                  smoothed_trajectory.phi, "r.-");
  plt::grid(true);
  plt::xlabel("s(m)");
  plt::ylabel("heading(rad)");

  plt::show();
} // namespace planning
} // namespace planning
} // namespace apollo
